#include <iostream>
#include "transitHub.h"
#include "robotModel.h"
#include "gaitStep.h"
#include <pthread.h>
#include <map>
using namespace std;

transitHub* port=transitHub::getInstance();
pthread_t ntid;
bool is_end=0;
string cmd_list;
double dt=10;
map<char,GaitStep*> gaits;

void * create_thread(void *arg)
{
    string t_cmd_list;
    while(!is_end)
    {
        t_cmd_list=cmd_list;
        for(int i=0;i<t_cmd_list.size();i++)
        {
            char cmd=t_cmd_list[i];
            GaitStep* g;
            g=gaits[cmd];
            int data[20];
             for(int ii=0;ii<g->getLen();ii++)
            {
                g->getData(data,ii);
                port->doLoopTx(data);
                usleep(dt*1000);
            }

        }
    }

}
void gaitInit()
{
    GaitStep* g;
    g=new GaitStep("walk");
    if(g->loadFile("walk"))
        gaits['w']=g;

    g=new GaitStep("leftkick");
    if(g->loadFile("leftkick"))
        gaits['k']=g;

}

int main() {
    if(!port->portOpen(1,57600))
        cout<<"open port error!\n";
     int err;
     gaitInit();
    err = pthread_create(&ntid, NULL, create_thread, NULL);
    string msg="w";
    cmd_list="w";
    while(1)
    {
       cin>>msg;
        cmd_list=msg;
    }
    return 0;
}
